#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>

#include <pcl/ros/conversions.h>
#include <pcl/point_types.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>

#include <pcl/io/pcd_io.h>
#include <pcl/ModelCoefficients.h>

#include <pcl/filters/passthrough.h>
#include <pcl/filters/extract_indices.h>

#include <pcl/features/normal_3d.h>

#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>

#include "tf/transform_broadcaster.h"
#include "tf/transform_listener.h"
#include "std_srvs/Empty.h"

ros::Publisher plane_pub;
ros::Publisher cylinder_pub;
ros::Publisher filter_pub;
int i = 0;
bool detect = false;
bool cylinder_found = false;

static double table_height = 0.72;
static tf::TransformBroadcaster* pBroadcaster = NULL;
static tf::TransformListener* pListener = NULL;
static tf::StampedTransform odom_camera_transform;
static tf::Transform cylinder_transform;
static sensor_msgs::PointCloud2::Ptr buffer (new sensor_msgs::PointCloud2);

bool startCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
{
	ROS_INFO("start detecting");
	detect = true;
	cylinder_found = false;	
	return true;
}

bool stopCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
{
	ROS_INFO("stop detecting");
	detect = false;
	cylinder_found = false;	
	return true;
}

void callback(const sensor_msgs::PointCloud2::ConstPtr& cloud)
{
	if(cylinder_found){
			pBroadcaster->sendTransform(tf::StampedTransform(cylinder_transform, ros::Time::now(), "/odom", "/cylinder_center"));	
	} 	

	if(i < 8 ){ //rate limiting
		i++;
		ros::spinOnce();
	} else if (!detect){
		i = 0;
	} else {
		i = 0;				

		// filter		
		pcl::PassThrough<sensor_msgs::PointCloud2> pass;
		pcl::ExtractIndices<pcl::PointXYZ> extract_indices;
		pcl::ExtractIndices<pcl::Normal> extract_normals;

		// Normal estimation
		pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimation;
		pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> segmentation_from_normals;

		// Create the segmentation object
		pcl::SACSegmentation<pcl::PointXYZ> seg;

		pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
		pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZ> ());
		pcl::search::KdTree<pcl::PointXYZ>::Ptr tree3 (new pcl::search::KdTree<pcl::PointXYZ> ());

		// The plane and cylinder coefficients
		pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
		pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients ());
		pcl::ModelCoefficients::Ptr coefficients_cylinder (new pcl::ModelCoefficients ());

		// The plane and cylinder inliers
		pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
		pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices ());
		pcl::PointIndices::Ptr inliers_cylinder (new pcl::PointIndices ());

		// The point clouds
		sensor_msgs::PointCloud2::Ptr passthrough_filtered (new sensor_msgs::PointCloud2);
		sensor_msgs::PointCloud2::Ptr plane_output_cloud (new sensor_msgs::PointCloud2);
		sensor_msgs::PointCloud2::Ptr cylinder_output_cloud (new sensor_msgs::PointCloud2);
		sensor_msgs::PointCloud2::Ptr cloud_transformed (new sensor_msgs::PointCloud2); 

		// The PointCloud
		pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr plane_cloud (new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr plane_output (new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr remove_transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr cylinder_cloud (new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr cylinder_output (new pcl::PointCloud<pcl::PointXYZ>);

		// The cloud normals
		pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal> ());        // for plane
		pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal> ());       // for cylinder
		pcl::PointCloud<pcl::Normal>::Ptr cloud_normals3 (new pcl::PointCloud<pcl::Normal> ()); 	  
		

		// Convert PointCloud2 to /odom frame
		try{		
			pListener->waitForTransform("/odom", (*cloud).header.frame_id, (*cloud).header.stamp, ros::Duration(10.0) );
			pcl_ros::transformPointCloud ("/odom", *cloud, *cloud_transformed, *pListener);
		}
	catch(tf::TransformException e){
			ROS_INFO("Transform_PointCloud: %s", e.what());
			return;
		}
		
		//Pass through Filtering
		pass.setInputCloud (cloud_transformed);	
		pass.setFilterFieldName ("x");
		pass.setFilterLimits (0.35, 0.55);
		pass.filter (*passthrough_filtered);

		pass.setInputCloud (passthrough_filtered);
		pass.setFilterFieldName ("y");
		pass.setFilterLimits (-0.08, 0.22);
		pass.filter (*passthrough_filtered);

		pass.setInputCloud (passthrough_filtered);
		pass.setFilterFieldName ("z");
		pass.setFilterLimits (0.70, 0.85);
		pass.filter (*passthrough_filtered);	

		//publish the filtered cloud
		filter_pub.publish(passthrough_filtered);	
		
		// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
		pcl::fromROSMsg (*passthrough_filtered, *plane_cloud);


		// Estimate point normals
		normal_estimation.setSearchMethod (tree2);
		normal_estimation.setInputCloud (plane_cloud);
		normal_estimation.setKSearch (25);
		normal_estimation.compute (*cloud_normals2);

		// Create the segmentation object for the planar model and set all the parameters
		segmentation_from_normals.setOptimizeCoefficients (true);
		segmentation_from_normals.setModelType (pcl::SACMODEL_NORMAL_PLANE);
		segmentation_from_normals.setMethodType (pcl::SAC_RANSAC);
		segmentation_from_normals.setNormalDistanceWeight (0.1);
		segmentation_from_normals.setMaxIterations (5000);
		segmentation_from_normals.setDistanceThreshold (0.06);
		segmentation_from_normals.setInputCloud (plane_cloud);
		segmentation_from_normals.setInputNormals (cloud_normals2);

		// Obtain the plane inliers and coefficients
		segmentation_from_normals.segment (*inliers_plane, *coefficients_plane);

		 // Extract the planar inliers from the input cloud
		extract_indices.setInputCloud (plane_cloud);
		extract_indices.setIndices (inliers_plane);
		extract_indices.setNegative (true);
		extract_indices.filter (*cylinder_cloud);
		extract_indices.setInputCloud (plane_cloud);
		extract_indices.setIndices (inliers_plane);
		extract_indices.setNegative (false);
		extract_indices.filter (*plane_output);
		extract_normals.setNegative (true);
		extract_normals.setInputCloud (cloud_normals2);
		extract_normals.setIndices (inliers_plane);
		extract_normals.filter (*cloud_normals3);
	
		//colorize plane cloud and publish it
		pcl::PointCloud<pcl::PointXYZRGB> plane_color;
		plane_color.width    = plane_output->width;
		plane_color.height   = plane_output->height;
		plane_color.is_dense = true;
		plane_color.points.resize (plane_color.width * plane_color.height);
		plane_color.sensor_origin_ = plane_output->sensor_origin_;
		plane_color.sensor_orientation_ = plane_output->sensor_orientation_;
		plane_color.header = plane_output->header;

		for(int i = 0; i < plane_color.size(); i++){
			plane_color.points[i].x = plane_output->points[i].x;
			plane_color.points[i].y = plane_output->points[i].y;		
			plane_color.points[i].z = plane_output->points[i].z;
			plane_color.points[i].r = 0;
			plane_color.points[i].g = 240;
			plane_color.points[i].b = 0;
		}

		pcl::toROSMsg (plane_color, *plane_output_cloud);
	 	plane_pub.publish(plane_output_cloud);
		
		// Create the segmentation object for cylinder segmentation and set all the openniparameters
		segmentation_from_normals.setOptimizeCoefficients (true);
		segmentation_from_normals.setModelType (pcl::SACMODEL_CYLINDER);
		segmentation_from_normals.setMethodType (pcl::SAC_RANSAC);
		segmentation_from_normals.setNormalDistanceWeight (0.1);
		segmentation_from_normals.setMaxIterations (5000);
		segmentation_from_normals.setDistanceThreshold (0.02);
		segmentation_from_normals.setRadiusLimits (0.02, 0.05);
		segmentation_from_normals.setInputCloud (cylinder_cloud);
		segmentation_from_normals.setInputNormals (cloud_normals3);

		// Obtain the cylinder inliers and coefficients
		segmentation_from_normals.segment (*inliers_cylinder, *coefficients_cylinder);

		// Publish the cylinder cloud
		extract_indices.setInputCloud (cylinder_cloud);
		extract_indices.setIndices (inliers_cylinder);
		extract_indices.setNegative (false);
		extract_indices.filter (*cylinder_output);

		if (cylinder_output->points.empty ()){
		   std::cerr << "Can't find the cylinder component." << std::endl;
			 return;		
		}
		
		//colorize the clyinder cloud and publish it
		pcl::PointCloud<pcl::PointXYZRGB> cylinder_color;
		cylinder_color.width    = cylinder_output->width;
		cylinder_color.height   = cylinder_output->height;
		cylinder_color.is_dense = true;
		cylinder_color.points.resize (cylinder_color.width * cylinder_color.height);
		cylinder_color.sensor_origin_ = cylinder_output->sensor_origin_;
		cylinder_color.sensor_orientation_ = cylinder_output->sensor_orientation_;
		cylinder_color.header = cylinder_output->header;

		for(int i = 0; i < cylinder_color.size(); i++){
			cylinder_color.points[i].x = cylinder_output->points[i].x;
			cylinder_color.points[i].y = cylinder_output->points[i].y;		
			cylinder_color.points[i].z = cylinder_output->points[i].z;
			cylinder_color.points[i].r = 238;
			cylinder_color.points[i].g = 0;
			cylinder_color.points[i].b = 0;
		}
		pcl::toROSMsg (cylinder_color, *cylinder_output_cloud);
		cylinder_pub.publish(cylinder_output_cloud);		

		//create a transform for the center of the cylinder
		tf::Transform transform;
		tf::Vector3 origin;
		origin.setX(coefficients_cylinder->values[0]);
		origin.setY(coefficients_cylinder->values[1]);
		origin.setZ(coefficients_cylinder->values[2]);
		transform.setOrigin(origin);
		transform.setRotation(tf::Quaternion(0,0,0));	

		double x_axis = coefficients_cylinder->values[3];
		double y_axis = coefficients_cylinder->values[4];
		double z_axis = coefficients_cylinder->values[5];
			
		ROS_INFO("x = %f", x_axis);
		ROS_INFO("y = %f", y_axis);
		ROS_INFO("z = %f", z_axis);
		
		//does the coordinate frame match our criteria
		bool publish = coefficients_cylinder->values[6] > 0;
		publish = publish && fabs(x_axis) < 0.2;
		publish = publish && fabs(y_axis) < 0.2;	
		publish = publish && fabs(z_axis) > 0.8;
		publish = publish && fabs(sqrt(x_axis*x_axis+y_axis*y_axis+z_axis*z_axis)) < 1.2;		
		publish = publish && transform.getOrigin().getX() > 0.35;
		publish = publish && transform.getOrigin().getX() < 0.60;
		publish = publish && transform.getOrigin().getY() > -0.08;
		publish = publish && transform.getOrigin().getY() < 0.22;
		publish = publish && transform.getOrigin().getZ() > table_height; 
		publish = publish && transform.getOrigin().getZ() < table_height+0.1;
	
		//publish the center of the cylinder as tf_frame
		if(publish){		
			ROS_INFO("New cylinder frame published");
			cylinder_found = true;
			tf::Vector3 transform_origin = transform.getOrigin();
			transform_origin.setZ(table_height + 0.05);
			transform.setOrigin(transform_origin);
			cylinder_transform = transform;
		}	
	}
}

int
main (int argc, char** argv)
{
  //INITIALIZE ROS
  ros::init (argc, argv, "extract_cylinder");
  ros::NodeHandle nh;
	pListener = new(tf::TransformListener);
	pBroadcaster = new(tf::TransformBroadcaster);
	ros::ServiceServer start_srv = nh.advertiseService("start_detecting_srv", startCallback);
	ros::ServiceServer stop_srv = nh.advertiseService("stop_detecting_srv", stopCallback);
	
  ros::Subscriber sub = nh.subscribe("cloud_throttled", 1, callback);
	filter_pub = nh.advertise<sensor_msgs::PointCloud2> ("filter_cloud", 1);
 	plane_pub = nh.advertise<sensor_msgs::PointCloud2> ("plane_cloud", 1);
  cylinder_pub = nh.advertise<sensor_msgs::PointCloud2> ("cylinder_cloud", 1);

  ros::spin();

  return (0);
}

